#include "CommandString.h"
#include "motor.h"

// 给定开头为指令的字符串，返回字符串长度（手写Strlen）
uint8_t GetCommandLen(char* Command)
{
	char* indexCommandLen = Command;
	uint8_t Len = 0;
	while(*indexCommandLen != '\0')
	{
		Len++;
		indexCommandLen++;
	}
	return Len;
}

// 给定开头为指令的字符串，返回$分隔符的位置（以0开头）
uint8_t FindSignalIndex(char* Command, char Signal)
{
	uint8_t indexCommand = 0, LenCommand = GetCommandLen(Command);
	while(indexCommand < LenCommand)
    {
        if(Command[indexCommand] == Signal) break;
        indexCommand ++;
    }
    return indexCommand;
}

// SpeedNumber means Compare Value
uint16_t GetSpeedNumber(char* NumberIndex)
{
	uint16_t SpeedNumber = 0;
	while(*NumberIndex >= '0' && *NumberIndex <= '9')
	{
		SpeedNumber = SpeedNumber * 10 + *NumberIndex - '0';
		NumberIndex ++;
	}
	return SpeedNumber;
}

// 给出开头即为Command的字符串，实现方向和速度设置
//"For","Bac","Lef","Rig","LeR","RiR"
void GetCommandAndPerform(char* Command)
{
    uint8_t SpaceSignal = FindSignalIndex(Command,' ');
    uint8_t DividSignal = FindSignalIndex(Command+SpaceSignal,'$');
	uint16_t SpeedNumber = GetSpeedNumber(Command+SpaceSignal+DividSignal+1);
/*    
    HAL_UART_Transmit_DMA(&huart4,(uint8_t*)"SpaceSignal:",12);
    osDelay(10);
    DigitalOutput(SpaceSignal);
    osDelay(10);
    HAL_UART_Transmit_DMA(&huart4,(uint8_t*)"DividSignal:",12);
    osDelay(10);
    DigitalOutput(DividSignal);
    osDelay(10);
    DigitalOutput(SpeedNumber);
    osDelay(10);
    
    HAL_UART_Transmit_DMA(&huart4,(uint8_t*)Command + SpaceSignal + 1,3);
    osDelay(10);
    
    DigitalOutput(memcmp(Command + SpaceSignal + 2, "For", 3*sizeof(char)));
    osDelay(10); 
*/    
    if(memcmp(Command + SpaceSignal + 1, CommandList[0], 3) == 0) {
        SetMotorDirectionForward();
        SetChassisSpeedByCCR(SpeedNumber);
        //HAL_UART_Transmit_DMA(&huart4,(uint8_t*)CommandList[0],3);
	//	puts(CommandList[0]);
	}
    if(memcmp(Command + SpaceSignal + 1, CommandList[1], 3) == 0) {
        SetMotorDirectionBackward();
        SetChassisSpeedByCCR(SpeedNumber);
        //HAL_UART_Transmit_DMA(&huart4,(uint8_t*)CommandList[1],3);
	//	puts(CommandList[1]);
    }
    if(memcmp(Command + SpaceSignal + 1, CommandList[2], 3) == 0) {
        SetMotorDirectionTurnLeft();
        SetChassisSpeedByCCR(SpeedNumber);
        //HAL_UART_Transmit_DMA(&huart4,(uint8_t*)CommandList[2],3);
	//	puts(CommandList[2]);
	}
    if(memcmp(Command + SpaceSignal + 1, CommandList[3], 3) == 0) {
        SetMotorDirectionTurnRight();
        SetChassisSpeedByCCR(SpeedNumber);
        //HAL_UART_Transmit_DMA(&huart4,(uint8_t*)CommandList[3],3);
	//	puts(CommandList[3]);
    }
	if(memcmp(Command + SpaceSignal + 1, CommandList[4], 3) == 0) {
        SetMotorDirectionLeftRevolve();
        SetChassisSpeedByCCR(SpeedNumber);
        //HAL_UART_Transmit_DMA(&huart4,(uint8_t*)CommandList[4],3);
	//	puts(CommandList[4]);
    }
    if(memcmp(Command + SpaceSignal + 1, CommandList[5], 3) == 0) {
        SetMotorDirectionRightRevolve();
        SetChassisSpeedByCCR(SpeedNumber);
        //HAL_UART_Transmit_DMA(&huart4,(uint8_t*)CommandList[5],3);
	//	puts(CommandList[5]);
    }
}
